import os
import subprocess
import rospy
from std_msgs.msg import String

# 动态加载ROS和自定义工作空间的环境变量
def load_ros_environment():
    ros_setup_path = "/opt/ros/noetic/setup.bash"  # 你的ROS版本路径
    mavros_ws_setup_path = "/home/wushuanglang/mavros_ws/devel/setup.bash"  # 自定义工作空间

    # 通过subprocess运行bash命令来source环境，并将环境变量写入Python的os.environ
    command = ['bash', '-c', f"source {ros_setup_path} && source {mavros_ws_setup_path} && env"]
    proc = subprocess.Popen(command, stdout=subprocess.PIPE)
    for line in proc.stdout:
        key, _, value = line.decode("utf-8").partition("=")
        os.environ[key] = value.strip()

# 发布消息到话题
def talker():
    # 初始化ROS节点
    rospy.init_node('talker', anonymous=True)
    # 定义一个发布者，发布到'chatter'话题，消息类型为String
    pub = rospy.Publisher('chatter', String, queue_size=10)
    rate = rospy.Rate(2)  # 设置发布频率为10Hz

    while not rospy.is_shutdown():
        # 要发布的消息内容
        hello_str = "Hello ROS from outside workspace! %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        # 发布消息
        pub.publish(hello_str)
        rate.sleep()

if __name__ == '__main__':
    # 加载ROS环境变量
    load_ros_environment()
    
    try:
        # 开始发布消息
        talker()
    except rospy.ROSInterruptException:
        pass

